#ifndef PROJECTOR
#define PROJECTOR
#include "sharedStructure.hpp"
#include "Eigen/Core"
#include <opencv2/core/core.hpp>

// functions to project from tan's coordinate system to Matlab image coordinate system
// specificaly, suppose image data is stored in this class
class projector{
public:

	 projector(){};
	 ~projector(){};
	
	bool initialize(float s, float* p_R, float* p_T,
					int num_orient, int num_length, int filter_size);
	bool release();
	
	bool updatePlane(float *p_u,float *p_v, float *p_ori,float *p_nt, float s, float width, float height);

	bool updateCamera(float *pP, float *direction, float zoom_x, float zoom_y); 
	void updataFilterParam(int num_orient,int num_length, int filter_size){n_orient = num_orient; n_length= num_length; fh=filter_size;};

	bool worldPoint2Image(float X, float Y, float Z, float &x, float &y);
	bool worldPoint2Tan(float X, float Y, float Z, float &Xt,float &Yt, float &Zt);
	bool tanPoint2World(float Xt, float Yt, float Zt, float &Xw, float &Yw, float &Zw);
	bool tanDirection2World(float Xt, float Yt, float Zt, float &Xw, float &Yw, float &Zw);
	bool tanPoint2Plane(float Xt, float Yt, float Zt, float &a, float &b);
	bool planePoint2World(float a, float b, float &X, float&Y,float &Z);
	bool planePoint2Tan(float a, float b, float &Xt, float &Yt, float &Zt);
	
	bool planePoint2Image(float xp, float yp, float &x, float &y);
	bool planePoint2ImageSlow(float xp, float yp, float &x, float &y);


	bool planeLine2Image(const line &plane_line, line &matlab_line);
	bool planeLineVertex2MatlabImage(const lineVertex &plane_line, lineVertex &matlab_line);

	bool isVisible();


	static bool estimatePandDir(const std::deque<pt_pair_s> &pts,float dest_P[][4],float cam_dir[3], float &error);
	bool estimatePByPlaneInfo(const std::deque<pt_pair_s> &pts,int img_width, int img_height, float dest_P[][4], float cam_dir[], float &error);
	void estimatePFast(const std::deque<pt_pair_s> &ptps, float dest_P[][4],float dir[3], float&error);
	bool estimateH(const std::deque<pt_pair_s> &pts, float dest_H[][3], float &error);
	static void testPespEstimation();
	static void testPdecomposition();

	static void decomposeP(float src_P[3][4], double rot[3][3], double cam_dir[3],double trans[3], double cparam[3][4]);
	static bool cam_direction(float srcP[3][4], float cam_dir[3]);
static bool estimatePerspectiveTransform(const std::deque<pt_pair_s> &ptps, float dest_P[][4],float dir[3], float&error);
private:
	// parameters for projection
	// X^T = s*R(X^W+T);

	// for the root tan
	float s_wt,inv_s_wt; // the scale from world to tan 
	float R[3][3];
	float inv_R[3][3];
	float T[3];

	// internal parameter
	float PRW[6];
	float constT[3];


	// for current terminal plane
	float u[3];
	float v[3];
	float plane_origin[3];
	float w,h; // width, height of plane
	float nt[4]; // nt of plane, saved version will be in tan coordiante system, the input from updataPlane is from tan coordiante system
	float n[3];// nt of plane, saved version will be in WORLD coordiante system, this is converted from nt in TAN coordinate system
	float t;
	float s_tp,inv_s_tp; // scale from tan to plane
	
	// for current camera
	float P[3][4];
	Eigen::MatrixXf Rwp;// affine transform from tan coorindate to plane coordinate
	float cam_dir[3];
	float zx,zy;
	
	
	// internal parameters for filters:
	int n_orient;
	int n_length;
	int fh;

    // caches for speeding up
    float *cache_cos,*cache_sin;
    
void invR();
void updatePreprocessedMatrix();
	

static bool estimateOrthognalTransform(const std::deque<pt_pair_s> &ptps, float dest_P[][4],float dir[3], float&error);
static bool cam_direction(Eigen::MatrixXf Pmat, Eigen::Vector3f &cam_dir);

bool worldPoint2Plane(float X, float Y, float Z, float &u, float &v, float &w);
bool projectH(float H[][3], float xr, float yr, float &xl, float &yl);
void initARToolkit( int xsize, int ysize );


void computePwp();
void fit2DLine(float x1, float y1, float x2, float y2, double n[3]);

static void cvFindPFast( const CvMat* objectPoints,
			   const CvMat* imagePoints, const CvMat* A,
			   const CvMat* distCoeffs, CvMat* rvec, CvMat* tvec,
                  int useExtrinsicGuess=0 );

};







#endif
